import time

from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster
import pos_nokov as pos
import sympy as sp
import math
import utils

field_range=[[-2100,2400],[-1300,1400]]
power=0
global poses, pos_ball

def led_red_solid(robot_group):
    """红灯常亮"""
    robot_group.led.set_led(led.COMP_ALL, 255, 1, 1, led.EFFECT_ON)

def led_grey_solid(robot_group):
    """grey常亮"""
    robot_group.led.set_led(led.COMP_ALL, 75, 255, 255, led.EFFECT_ON)

def get_pass_posi(pos_ball, pos_mate):
    bx=pos_ball[0]
    by=pos_ball[1]
    mx=pos_mate[0]
    my=pos_mate[1]
    x,y=sp.symbols('x, y')
    print('pos_ball')
    print(pos_ball)
    print('pos_mate')
    print(pos_mate)
    solutions=sp.nonlinsolve([(x-bx)*(by-my)-(y-by)*(bx-mx),
                              (x-bx)**2+(y-by)**2-120000],[x,y])
    # print(solutions)
    for s in solutions:
        if not ((bx<s[0]<mx or bx>s[0]>mx) and (by<s[1]<my or by>s[1]>my)):
            print('target posi:')
            print(s)
            posi=s
    ori=math.atan2(my-by, mx-bx)*180/math.pi
    # print('ori')
    # print(ori)
    return ori, posi
    
def move_to_shoot(robot_group):
    global power
    ori, posi=get_pass_posi(pos_ball, poses[1-power][1])
    posi_target=utils.trans_relative_co(poses[power], posi)
    print('move to:(y make a negative)')
    print(posi_target)
    robot_group.chassis.move(posi_target[0]/1000,-posi_target[1]/1000,0,0.6).wait_for_completed()
    
def rotate_to_shoot(robot_group):
    global power
    ori, posi=get_pass_posi(pos_ball, poses[1-power][1])
    z=ori-poses[power][0]
    print('rotate:')
    print(z)
    robot_group.chassis.move(0,0,z,45).wait_for_completed()
    
def shoot(robot_group):
    robot_group.chassis.move(0.4,0,0,1.5).wait_for_completed()
    
def back(robot_group):
    robot_group.chassis.move(-0.5,0,0,2).wait_for_completed()
    

def run():
    global poses, pos_ball, power
    # robomaster.config.LOCAL_IP_STR = "192.168.1.111"
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', 
                      '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()

    number = multi_robots.number_id_by_sn([5, robots_sn_list[4]], [6, robots_sn_list[5]])
    print("The number of robot is: {0}".format(number))
    g_all = multi_robots.build_group([5, 6])
    g_red = multi_robots.build_group([5])
    g_grey = multi_robots.build_group([6])
    groups=[g_red, g_grey]
    multi_robots.run([g_grey, led_grey_solid])
    multi_robots.run([g_red, led_red_solid])
    
    while(input("robot loop: Press q to quit\n") != "q"):
        poses=[pos.get_device_pose('EP5'),pos.get_device_pose('EP6')]
        print(poses)
        # pos2=pos.get_device_pose('EP2')
        # pos3=pos.get_device_pose('EP3')
        pos_ball=pos.get_ball_pose()
        print('ball pose:')
        print(pos_ball)
        print('current agent pose:')
        print(poses[power])
        multi_robots.run([groups[power],move_to_shoot])
        multi_robots.run([groups[power],rotate_to_shoot])
        multi_robots.run([groups[power],shoot])
        multi_robots.run([groups[power],back])
        power=1-power
        
    
    multi_robots.close()
    